#include "rclcpp/rclcpp.hpp"                  // ROS2核心头文件 [变化点1]
#include "param_space/msg/param_changes.hpp"  // 消息头文件路径变更 [变化点2]
#include "param_space/srv/set_param.hpp"      // 服务头文件路径变更 [变化点3]
#include "param_space/srv/get_param.hpp"
#include <unordered_map>
#include <string>
#include <mutex>                              // 添加线程安全支持 [变化点4]

using namespace std::chrono_literals;

class ParamSpaceNode : public rclcpp::Node {  // 必须继承自Node类 [变化点5]
public:
  ParamSpaceNode() : Node("param_space_server") {
    // 初始化服务 [变化点6]
    srv_set_ = create_service<param_space::srv::SetParam>(
      "/set_param",
      [this](const std::shared_ptr<param_space::srv::SetParam::Request> req,
             std::shared_ptr<param_space::srv::SetParam::Response> res) {
        this->set_param_callback(req, res);
      });
    
    srv_get_ = create_service<param_space::srv::GetParam>(
      "/get_param",
      [this](const std::shared_ptr<param_space::srv::GetParam::Request> req,
             std::shared_ptr<param_space::srv::GetParam::Response> res) {
        this->get_param_callback(req, res);
      });

    // 初始化发布者 [变化点7]
    pub_changes_ = create_publisher<param_space::msg::ParamChanges>(
      "/param_changes", 10);
    
    // 定时器替代spinOnce [变化点8]
    timer_ = create_wall_timer(
      100ms, [this]() { /* ROS2无需主动spin */ });
  }

private:
  // 服务回调函数签名变更 [变化点9]
  void set_param_callback(
    const std::shared_ptr<param_space::srv::SetParam::Request> req,
    std::shared_ptr<param_space::srv::SetParam::Response> res) 
  {
    std::lock_guard<std::mutex> lock(mutex_); // 线程安全处理
    if (req->keys.size() != req->vals.size()) {
      res->opcode = -1;
      res->msg = "key-value size mismatch!";
      return;
    }

    for (size_t i = 0; i < req->keys.size(); ++i) {
      parameters_[req->keys[i]] = req->vals[i];
    }

    auto msg = param_space::msg::ParamChanges();
    msg.changed_params = req->keys;
    pub_changes_->publish(msg);

    res->opcode = 0;
    res->msg = "success";
  }

  void get_param_callback(
    const std::shared_ptr<param_space::srv::GetParam::Request> req,
    std::shared_ptr<param_space::srv::GetParam::Response> res) 
  {
    std::lock_guard<std::mutex> lock(mutex_);
    for (const auto& key : req->keys) {
      res->vals.push_back(parameters_[key]);
    }
  }

  // 成员变量 [变化点10]
  std::unordered_map<std::string, std::string> parameters_;
  rclcpp::Service<param_space::srv::SetParam>::SharedPtr srv_set_;
  rclcpp::Service<param_space::srv::GetParam>::SharedPtr srv_get_;
  rclcpp::Publisher<param_space::msg::ParamChanges>::SharedPtr pub_changes_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::mutex mutex_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);                     // ROS2初始化 [变化点11]
  auto node = std::make_shared<ParamSpaceNode>();
  rclcpp::spin(node);                          // 新的spin机制 [变化点12]
  rclcpp::shutdown();
  return 0;
}
